#include "MotionReaderC3D.h"

#include <string>
#include <boost/algorithm/string.hpp>
#include <iostream>

#define DETAILED_INFORMATION

using namespace std;

namespace MMM
{


MotionReaderC3D::MotionReaderC3D() = default;

MotionReaderC3D::~MotionReaderC3D() = default;

float MotionReaderC3D::convertFloat(char mem_float[4]) {
	switch ((int)ptype) {
	case 84:
		return *(float*) mem_float;
	case 85:
		return ConvertDecToFloat(mem_float);
	case 86:
		MMM_WARNING << "MIPS is not supported yet." <<endl;
		return -1;
	default:
		MMM_WARNING << "unknown processor type." << endl;
		return -1;
	}
}

//Utilities to convert floating points to and from the DEC file format used by Vicon.
//Records are 256 (16 bit) words long.
float MotionReaderC3D::ConvertDecToFloat(char bytes[4]) {
	char p[4];
	p[0] = bytes[2];
	p[1] = bytes[3];
	p[2] = bytes[0];
	p[3] = bytes[1];
	if (p[0] || p[1] || p[2] || p[3]) {
		--p[3]; // adjust exponent
	}

	return *(float*) p;
}

void MotionReaderC3D::ConvertFloatToDec(float f, char* bytes) {
	char* p = (char*) &f;
	bytes[0] = p[2];
	bytes[1] = p[3];
	bytes[2] = p[0];
	bytes[3] = p[1];
	if (bytes[0] || bytes[1] || bytes[2] || bytes[3]) {
		++bytes[1]; // adjust exponent
	}
}

long MotionReaderC3D::getFilePositionGroup(const char* group) {
    // Start at Byte 512 of infile
    // First four bytes specified
    return  getFilePositionParams(512+4, group);
}

long MotionReaderC3D::getFilePositionParams(long group_pos, const char* parameters) {

    char pname[25];
    long pbyte;
    infile.clear();
	infile.seekg(group_pos, ios::beg);

    // Scan for each parameter of interest in group POINT
    while (strncmp(pname,parameters,strlen(parameters)) != 0 && infile)
    {
        infile.read(reinterpret_cast<char *>(&pname),sizeof(char)*strlen(parameters));
        infile.seekg(-1*static_cast<int>((strlen(parameters)-1)*sizeof(char)),ios::cur);
    }

    if (!infile.eof()) {
        // reposition to end of LABELS
        infile.seekg((strlen(parameters)-1)*sizeof(char),ios::cur);

        // Record this file position
        pbyte = (long)infile.tellg();

#ifdef DEBUG
    cout << "\tfound parameter " << parameters << endl;
#endif

    } else {
        pbyte = -1;
    }
    return pbyte;
}

// Reads in parameter records from C3D file passed
// All data in header is specified as "16 bit words", or the equivalent of one unsigned char
std::vector<std::string> MotionReaderC3D::Read_C3D_Marker_Labels() {

    std::vector<std::string> labels(num_markers);

	char dim, type;
	unsigned char nrow, ncol;
	unsigned int i, j;
	int offset;
	long gbyte, pbyte;

	char **prefixes;
	unsigned int num_prefixes = 0;
	signed short is_prefixes = 0;

	if (infile.is_open() ) {

		// Because it is unknown how many groups and how many
		// parameters/group are stored, must scan for each variable
		// of interest.

		// 1st scan for group SUBJECT
		// Parameters stored in SUBJECTS are:
		//		1. USES_PREFIXES
		//		2. LABEL_PREFIXES

        gbyte = getFilePositionGroup("SUBJECT");

		if (gbyte > 0) {

			pbyte = getFilePositionParams(gbyte, "USES_PREFIXES");

			if (pbyte > 0) {
                infile.clear();
				infile.seekg(pbyte,ios::beg);
				RPF(&offset, &type, &dim);

				infile.read(reinterpret_cast<char *>(&is_prefixes), sizeof(is_prefixes)*1);

				if (is_prefixes == 1) {
					pbyte = getFilePositionParams(gbyte, "LABEL_PREFIXES");

					if (pbyte > 0) {
						infile.seekg(pbyte,ios::beg);
						RPF(&offset, &type, &dim);
						infile.read(reinterpret_cast<char *>(&ncol), sizeof(char)*1);
						infile.read(reinterpret_cast<char *>(&nrow), sizeof(char)*1);

						num_prefixes = nrow;
						prefixes = new char*[nrow];
						for (i=1;i<=num_prefixes;i++)
						{
							prefixes[i-1] = new char[ncol];
							for (j=1;j<=ncol;j++)
							{
								infile.read(&prefixes[i-1][j-1],sizeof(char)*1);
								if (prefixes[i-1][j-1] == ' ') {
									prefixes[i-1][j-1] = 0;
								}
							}
						}

					}
				}
			}

		}

		// 2nd scan for group POINT
		// Parameters stored in POINT are:
		//		1. LABELS
		//		2. DESCRIPTIONS
		//		3. USED
		//		4. UNITS
		//		5. SCALE
		//		6. RATE
		//		7. DATA_START
		//		8. FAMES
		//		9. INITIAL_COMMAND
		//		10.X_SCREEN
		//		11.Y_SCREEN

        gbyte = getFilePositionGroup("POINT");

		if (gbyte > 0) {

			pbyte = getFilePositionParams(gbyte, "LABELS");

			if (pbyte > 0) {
                infile.clear();
				infile.seekg(pbyte,ios::beg);

				RPF(&offset, &type, &dim); // dim should be 2 for a 2D array of labels[np][4]
				// read in array dimensions: should be 4 x np
				infile.read(reinterpret_cast<char *>(&ncol), sizeof(char)*1);
				infile.read(reinterpret_cast<char *>(&nrow), sizeof(char)*1);

				for (i=1;i<=nrow;i++)
				{
					//lables[i-1] = new char[ncol];
					char *label = new char[ncol];
					for (j=1;j<=ncol;j++)
					{
						infile.read(reinterpret_cast<char *>(&label[j-1]),sizeof(char)*1);
						if (label[j-1] == ' ') {
							label[j-1] = 0;
						}
					}
					labels[i-1] = label;
					delete []label;
				}
			}

			if (num_markers > 255) {

				pbyte = getFilePositionParams(gbyte, "LABELS2");

				if (pbyte > 0) {
                    infile.clear();
					infile.seekg(pbyte,ios::beg);

					RPF(&offset, &type, &dim); // dim should be 2 for a 2D array of labels[np][4]
					// read in array dimensions: should be 4 x np
					infile.read(reinterpret_cast<char *>(&ncol), sizeof(char)*1);
					infile.read(reinterpret_cast<char *>(&nrow), sizeof(char)*1);

					for (i=1;i<=nrow;i++)
					{
						//mlabels[i-1+255] = new char[ncol];
						char *label = new char[ncol];
						for (j=1;j<=ncol;j++)
						{
							infile.read(reinterpret_cast<char *>(&label[j-1]),sizeof(char)*1);
							if (label[j-1] == ' ') {
								label[j-1] = 0;
							}
						}
						labels[i-1+255] = label;
						delete []label;
					}
				}
			}
		}
	}
    return labels;
}

void MotionReaderC3D::RPF(int *offset, char *type, char *dim) {
	char offset_low, offset_high;
	// Read Parameter Format for the variable following the
	// parameter name
	//		offset = number of bytes to start of next parameter (2 Bytes, reversed order: low, high)
	//		T = parameter type: -1 = char, 1 = boolean, 2 = int, 3 = float
	//		D = total size of array storage (incorrect, ignore)
	//		d = number of dimensions for an array parameter (d=0 for single value)
	//		dlen = length of data
	infile.read(&offset_low, sizeof(char)*1); // byte 1
	infile.read(&offset_high, sizeof(char)*1); // byte 2
	*offset = 256* offset_high + offset_low;
	infile.read(type, sizeof(char)*1); // byte 3
	infile.read(dim, sizeof(char)*1); // byte 4
}

//Reads in header information from C3D file passed
void MotionReaderC3D::Read_C3D_Header() {

	unsigned short key1, max_gap;
	char cdum;
	char mem_float[4];

	if (infile.is_open()) {

		// Read processor type
		infile.seekg(512, ios::beg);

		// First four bytes specified
		// bytes 1 and 2 are part of ID key (1 and 80)
		infile.read(&cdum, sizeof(char)* 1); // byte 1
		infile.read(&cdum, sizeof(char)*1); // byte 2
		// byte 3 holds # of parameter records to follow
		infile.read(&cdum, sizeof(char)*1); // byte 3
		// byte 4 is processor type, Vicon uses DEC (type 2)
		infile.read(reinterpret_cast<char *>(&ptype), sizeof(char)*1); // byte 3

		/* Read in Header */
		infile.seekg(0, ios::beg);
		// Key1, byte = 1,2; word = 1
		infile.read(reinterpret_cast<char *>(&key1), sizeof(key1)*1);

		// Number of 3D points per field, byte = 3,4; word = 2
		infile.read(reinterpret_cast<char *>(&num_markers), sizeof(num_markers) *1);

        // Number of analog channels per field byte = 5,6; word = 3
		infile.read(reinterpret_cast<char *>(&num_channels), sizeof(num_channels)*1);

		// Field number of first field of video data, byte = 7,8; word = 4
		infile.read(reinterpret_cast<char *>(&first_field), sizeof(first_field)*1);

		// Field number of last field of video data, byte = 9,10; word = 5
		infile.read(reinterpret_cast<char *>(&last_field), sizeof(last_field)*1);

		// Maximum interpolation gap in fields, byte = 11,12; word = 6
		infile.read(reinterpret_cast<char *>(&max_gap), sizeof(max_gap)*1);

		// Scaling Factor, bytes = 13,14,15,16; word = 7,8
		infile.read(&mem_float[0], sizeof(char)*1); // 1st byte
		infile.read(&mem_float[1], sizeof(char)*1); // 2nd byte
		infile.read(&mem_float[2], sizeof(char)*1); // 3rd byte
		infile.read(&mem_float[3], sizeof(char)*1); // 4th byte

		scale_factor = convertFloat(mem_float);

		// Starting record number, byte = 17,18; word = 9
		infile.read(reinterpret_cast<char *>(&start_record_num), sizeof(start_record_num)*1);

		// Number of analog frames per video field, byte = 19,20; word = 10
		infile.read(reinterpret_cast<char *>(&frames_per_field), sizeof(frames_per_field)*1);

		// Analog channels sampled
		if (frames_per_field != 0)
			num_channels /= frames_per_field;

		// Video rate in Hz, bytes = 21,22,23,24; word = 11,12
		infile.read(&mem_float[0], sizeof(char)*1); // 1st byte
		infile.read(&mem_float[1], sizeof(char)*1); // 2nd byte
		infile.read(&mem_float[2], sizeof(char)*1); // 3rd byte
		infile.read(&mem_float[3], sizeof(char)*1); // 4th byte

		video_rate =  convertFloat(mem_float);

#ifdef DEBUG
		// debug output
		cout << "Number of Markers:" << num_markers <<endl;
		cout << "Number of Channels: " << num_channels << endl;
		cout << "First field: " << first_field << endl;
		cout << "Last Field: " << last_field << endl;
		cout << "Scale Factor: " << scale_factor << endl;
		cout << "Start Record Number: " << start_record_num << endl;
		cout << "Frames per Field: " << frames_per_field << endl;
		cout << "Video Rate: " << video_rate << endl;
		cout << "Processor Type: ";
		switch ((int)ptype) {
		case 84:
			cout << "Intel" << endl;
			break;
		case 85:
			cout << "DEC" << endl;
			break;
		case 86:
			cout << "MIPS (not supported)" << endl;
			break;
		default:
			cout << "unknown" << endl;
		}
#endif
	}
}

// Reads in 3D position data from C3D file passed
// All data in header is specified as "16 bit words", or the equivalent of one unsigned char
std::vector<Eigen::Vector3f> MotionReaderC3D::Read_C3D_Marker(unsigned short marker_num)
{
	unsigned short frame_length, frame_num, i, offset;
	char cam;

    std::vector<Eigen::Vector3f> val(getNumFrames());
    for (int i = 0; i < getNumFrames(); i++) {
        val[i].setZero();
    }

	if (infile.is_open()) {
		// Data is stored in the following format
		// Each frame (or field) from first_field to last_field
		//		Each marker from 1 to num_markers
		//			Data: X,Y,Z,R,C (Total of 8 bytes)
		//		Next marker
		//		Each analog sample per frame from 1 to analog_frames_per_field
		//			Each analog channel from 1 to num_analog_channels
		//				Data: A (total of 2 bytes)
		//			Next channel
		//		Next sample
		// Next frame

		char data_length;
		char analog_bytes;

		if (scale_factor < 0.0) {
			data_length = 16;
			analog_bytes = 4;
		} else {
			data_length = 8;
			analog_bytes = 2;
		}

		// Startbyte is the starting location of tvd/adc data
		long start_byte = 512 * (start_record_num -1);

		// Determine data offset based upon starting channel number
		offset = data_length* (marker_num -1);

		// Determine interval to next frame of this markers data
		frame_length = (data_length*(num_markers-1))+(analog_bytes*num_channels*frames_per_field);

		// Position cursor to first data point
        infile.clear();
		infile.seekg(start_byte+offset,ios::beg);

		char mem_float[4];

		for (frame_num = first_field; frame_num <= last_field; frame_num++)
		{
			int index = frame_num-first_field;
			// Read XYZ data for this frame
			for (i=1;i<=3;i++) // for x,y,z
			{
				if (scale_factor < 0.0) {
					infile.read(&mem_float[0], sizeof(char)*1); // 1st byte
					infile.read(&mem_float[1], sizeof(char)*1); // 2nd byte
					infile.read(&mem_float[2], sizeof(char)*1); // 3rd byte
					infile.read(&mem_float[3], sizeof(char)*1); // 4th byte
					val[index][i-1] = convertFloat(mem_float);

				} else {
					short pos;
					infile.read(reinterpret_cast<char *>(&pos), sizeof(pos)* 1);
					val[index][i-1] = pos*scale_factor;
				}
			}

			if (scale_factor < 0.0) {
				infile.read(&mem_float[0], sizeof(char)*1); // 1st byte
				infile.read(&mem_float[1], sizeof(char)*1); // 2nd byte
				infile.read(&mem_float[2], sizeof(char)*1); // 3rd byte
				infile.read(&mem_float[3], sizeof(char)*1); // 4th byte

				//TODO convert data in order to get residual and # cameras

			} else {
				// Read Residual
				char c;
				infile.read(&c, sizeof(char)* 1);

				// Read # cameras
				infile.read(&cam, sizeof(char)* 1);
			}
			// Skip to the next frame
			infile.seekg(frame_length,ios::cur);
		}

#ifdef DEBUG
		cout << endl << "Marker Data" << endl;
		for (int j=0; j<5;j++) {
			cout << "frame: " << j+first_field << endl;
			cout << " x: " << val[j][0] << endl;
			cout << " y: " << val[j][1] << endl;
			cout << " z: " << val[j][2] << endl;
		}
#endif
	}

    return val;
}

PrefixMarkerDataPtr MotionReaderC3D::loadC3D(const std::string filename) {
	this->filename = filename;
	last_field = 0;
	first_field = 0;
    num_markers = 0;
	video_rate = 120;
    marker = PrefixMarkerDataPtr(new PrefixMarkerData());
	infile.open((char*)filename.c_str(), ios::in | ios::binary);
	Read_C3D_Header();

    std::vector<std::string> labels = Read_C3D_Marker_Labels();

    for (int i = 0; i < num_markers; i++)
    {
        std::vector<Eigen::Vector3f> markerData = Read_C3D_Marker(i + 1);

        std::vector<std::string> label;
        boost::split(label, labels[i], boost::is_any_of(":"));
        std::string marker_name = std::string();
        std::string prefix = std::string();
        if (label.size() == 2) {
            marker_name = label[1];
            prefix = label[0];
        }
        else if (label.size()) marker_name = label[0];
        else MMM_ERROR << "MarkerLabel " << labels[i] << " contains more than one ':'" << std::endl;
        if (marker_name.at(0) == '*') marker_name = std::string();

        for (int j = 0; j < getNumFrames(); j++) {
            if (!marker_name.empty() || !markerData[j].isZero())
                marker->addMarkerData((float) j / video_rate, markerData[j], marker_name, prefix);
        }
    }

    return marker;
}

int MotionReaderC3D::getNumFrames() {
    return last_field - first_field + 1;
}


}

